#include "AS5600.h"

void AS5600Init(){
    Wire.begin(FOC_CH1_SDA_PIN,FOC_CH1_SCL_PIN,FOC_CH1_FREQ);
    Wire1.begin(FOC_CH2_SDA_PIN,FOC_CH2_SCL_PIN,FOC_CH2_FREQ);
    
}
bool checkEncoderCH1(void){
    /* Read Low Byte */
    Wire.beginTransmission(AS5600_IIC_ADDRESS);
    Wire.write(AS5600_RAW_LOW);
    Wire.endTransmission();
    Wire.requestFrom(AS5600_IIC_ADDRESS, 1);
    int time = millis();
    while(Wire.available() == 0){
        if(millis()-time>100) return false;
    }
    return true;
}
bool checkEncoderCH2(void){
    /* Read Low Byte */
    Wire1.beginTransmission(AS5600_IIC_ADDRESS);
    Wire1.write(AS5600_RAW_LOW);
    Wire1.endTransmission();
    Wire1.requestFrom(AS5600_IIC_ADDRESS, 1);
    int time = millis();
    while(Wire1.available() == 0){
        if(millis()-time>100) return false;
    }
    return true;
}
unsigned int getRowFromAS5600_CH1(void){
    unsigned int retVal = -1;
    /* Read Low Byte */
    Wire.beginTransmission(AS5600_IIC_ADDRESS);
    Wire.write(AS5600_RAW_LOW);
    Wire.endTransmission();
    Wire.requestFrom(AS5600_IIC_ADDRESS, 1);
    while(Wire.available() == 0);
    int low = Wire.read();
    /* Read High Byte */  
    Wire.beginTransmission(AS5600_IIC_ADDRESS);
    Wire.write(AS5600_RAW_HIGH);
    Wire.endTransmission();
    Wire.requestFrom(AS5600_IIC_ADDRESS, 1);
    while(Wire.available() == 0);
    unsigned int high = Wire.read();
    high = high << 8;
    retVal = high | low;
    return retVal;
}
unsigned int getRowFromAS5600_CH2(void){
    unsigned int retVal = -1;
    /* Read Low Byte */
    Wire1.beginTransmission(AS5600_IIC_ADDRESS);
    Wire1.write(AS5600_RAW_LOW);
    Wire1.endTransmission();
    Wire1.requestFrom(AS5600_IIC_ADDRESS, 1);
    while(Wire1.available() == 0);
    int low = Wire1.read();
    /* Read High Byte */  
    Wire1.beginTransmission(AS5600_IIC_ADDRESS);
    Wire1.write(AS5600_RAW_HIGH);
    Wire1.endTransmission();
    Wire1.requestFrom(AS5600_IIC_ADDRESS, 1);
    while(Wire1.available() == 0);
    unsigned int high = Wire1.read();
    high = high << 8;
    retVal = high | low;
    return retVal;
}